/*
 * test_node.h
 *
 *  Created on: 16.09.2013
 *      Author: alex
 */
#ifndef TEST_NODE_H_
#define TEST_NODE_H_

#include <vector>
#include <bullet/LinearMath/btVector3.h>
#include <ros/node_handle.h>
#include <ros/timer.h>
//#include <traverse_analysis/traverse_analysis.h>

namespace bullet_rviz_test
{

class RvizMotionState;
class Heightfield;
typedef boost::shared_ptr<Heightfield> HeightfieldPtr;
class HeightfieldVisualization;
typedef boost::shared_ptr<HeightfieldVisualization> HeightfieldVisualizationPtr;
class Robot;
typedef boost::shared_ptr<Robot> RobotPtr;
class RobotVisualization;
typedef boost::shared_ptr<RobotVisualization> RobotVisualizationPtr;
class Physics;
typedef boost::shared_ptr<Physics> PhysicsPtr;
class PhysicalObject;

class TestNode
{
public:
  static int main(int argc, char ** argv);

private:
  struct RobotInfo {
    RobotPtr robot_;
    RobotVisualizationPtr visualization_;
    btVector3 initial_position_;
  };
  typedef boost::shared_ptr<RobotInfo> RobotInfoPtr;

  TestNode();

  int run();
  void resetRobots();
  void computeCosts(RobotInfo * robot_info);
  void createMockHeightfield();
  void createHeightfield();
  void resetTimerCallback(const ros::TimerEvent &);
  void robotMotionCallback(RobotInfo * robot_info);

  ros::NodeHandle nh_;
  ros::Timer reset_timer_;

  PhysicsPtr physics_;
  HeightfieldPtr heightfield_;
  HeightfieldVisualizationPtr heightfield_visualization_;
  std::vector<RobotInfoPtr> robots_;
  //boost::shared_ptr<traverse_analysis::TraverseAnalysis> traverse_analysis_;
};

}

#endif /* TEST_NODE_H_ */
